#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/PositionTarget.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/BatteryState.h>
#include <serial/serial.h>
#include <thread>
#include <mutex>
#include <queue>
#include <chrono>
#include <cstring>
#include <iomanip>
#include <sstream>

// 半精度浮点数转换函数
uint16_t floatToHalf(float f) {
    uint32_t x = *reinterpret_cast<uint32_t*>(&f);

    uint32_t sign = (x >> 16) & 0x8000;     // sign bit
    int32_t exp = ((x >> 23) & 0xFF) - 127; // exponent
    uint32_t mant = x & 0x7FFFFF;           // mantissa

    if (exp == 128) { // NaN or Inf
        if (mant == 0) {
            // Inf
            return sign | 0x7C00;
        } else {
            // NaN
            return sign | 0x7C00 | (mant >> 13);
        }
    }

    if (exp > 15) {
        // Overflow - set to Inf
        return sign | 0x7C00;
    } else if (exp > -15) {
        // Normalized number
        exp += 15;
        mant = mant >> 13;
        return sign | (exp << 10) | mant;
    } else if (exp >= -24) {
        // Subnormal number
        mant |= 0x800000;  // add leading 1
        int shift = (-14 - exp);
        mant = mant >> (shift + 13);
        return sign | mant;
    } else {
        // Underflow - set to zero
        return sign;
    }
}

float halfToFloat(uint16_t h) {
    uint32_t x = ((h & 0x8000) << 16) | // 符号位
                 (((h & 0x7c00) + 0x1c000) << 13) | // 指数
                 ((h & 0x03ff) << 13); // 尾数
    return *reinterpret_cast<float*>(&x);
}

struct UAVPositionData {
    uint32_t target_address;
    uint8_t uav_id;
    geometry_msgs::PoseStamped mavros_pose;
    geometry_msgs::PoseStamped detect_pose;
    uint8_t battery_percentage; // 电池百分比 (0-100)
    uint8_t status_flags;       // 状态位
    uint8_t reserved1;          // 保留字节1
    uint8_t reserved2;          // 保留字节2
    std::chrono::steady_clock::time_point timestamp;
};

struct ReceivedData {
    uint32_t source_address;
    uint8_t uav_id;
    geometry_msgs::PoseStamped mavros_pose;
    geometry_msgs::PoseStamped detect_pose;
    uint8_t battery_percentage; // 电池百分比
    uint8_t status_flags;       // 状态位
    uint8_t reserved1;          // 保留字节1
    uint8_t reserved2;          // 保留字节2
};

struct ReceivedControlData {
    uint8_t uav_id;
    float v_x, v_y, v_z, yaw;
    float reserved1, reserved2;
    int32_t mode;
};

class UAVSwarmCommunication {
private:
    ros::NodeHandle nh_;
    
    // 订阅者
    ros::Subscriber mavros_pose_sub_;
    ros::Subscriber detect_pose_sub_;
    ros::Subscriber battery_sub_; // 电池状态订阅
    
    // 发布者
    std::vector<ros::Publisher> other_uav_mavros_pubs_;
    std::vector<ros::Publisher> other_uav_detect_pubs_;
    ros::Publisher control_velocity_pub_;  // 发布控制速度
    ros::Publisher control_mode_pub_;      // 发布控制模式
    
    // 串口通信
    serial::Serial serial_port_;
    std::string serial_port_name_;  // 保存串口设备名
    int serial_baudrate_;           // 保存波特率
    
    // 数据缓存
    geometry_msgs::PoseStamped current_mavros_pose_;
    geometry_msgs::PoseStamped current_detect_pose_;
    bool mavros_pose_received_;
    bool detect_pose_received_;
    bool TX_debug, RX_debug;
    
    // 电池状态
    uint8_t battery_percentage_;
    uint8_t status_flags_;
    uint8_t reserved1_;
    uint8_t reserved2_;
    
    // 线程和同步
    std::thread send_thread_;
    std::thread receive_thread_;
    std::mutex pose_mutex_;
    std::mutex battery_mutex_;
    std::mutex serial_mutex_;
    
    // 配置参数
    uint32_t ground_station_addr_;
    std::vector<uint32_t> other_uav_addrs_;
    uint8_t this_uav_id_;
    
    // 话题名称参数
    std::string mavros_pose_topic_;
    std::string detect_pose_topic_;
    std::string battery_topic_;
    std::vector<std::string> other_uav_mavros_topics_;
    std::vector<std::string> other_uav_detect_topics_;
    
    // 频率参数
    double ground_station_freq_;
    double other_uav_freq_;
    double receive_freq_;
    
    // 定时器
    std::chrono::steady_clock::time_point last_ground_send_;
    std::chrono::steady_clock::time_point last_uav_send_;
    
    bool running_;

    // 数据包大小常量
    static const size_t PACKET_SIZE_SEND = 41;      // 发送包大小 (37+4=41) 3地址 + 2帧头 + 1ID + 28数据 + 2电池/状态 + 2待定 + 1校验 + 2帧尾 = 41字节
    static const size_t PACKET_SIZE_NORMAL = 38;    // 普通接收包大小 (34+4=38) 2帧头 + 1ID + 28数据 + 2电池/状态 + 2待定 + 1校验 + 2帧尾 = 38字节
    static const size_t PACKET_SIZE_CONTROL = 19;   // 控制包大小 (ID 04) (15+4=19) 2帧头 + 1ID + 13数据 + 2待定 + 1校验 + 2帧尾 = 19字节

public:
    UAVSwarmCommunication() : nh_("~"), mavros_pose_received_(false), detect_pose_received_(false), running_(true),
                              battery_percentage_(100), status_flags_(0), reserved1_(0), reserved2_(0) {
        loadParameters();
        initializeSerial();
        initializeROS();
        
        send_thread_ = std::thread(&UAVSwarmCommunication::sendThread, this);
        receive_thread_ = std::thread(&UAVSwarmCommunication::receiveThread, this);
        
        last_ground_send_ = std::chrono::steady_clock::now();
        last_uav_send_ = std::chrono::steady_clock::now();
        
        ROS_INFO("UAV Swarm Communication Node Started (Optimized with Half Precision)");
    }
    
    ~UAVSwarmCommunication() {
        running_ = false;
        if (send_thread_.joinable()) send_thread_.join();
        if (receive_thread_.joinable()) receive_thread_.join();
        if (serial_port_.isOpen()) serial_port_.close();
    }

private:
    void loadParameters() {
        int ground_addr_int, this_uav_id_int;
        nh_.param<int>("ground_station_addr", ground_addr_int, 0x000417);
        nh_.param<int>("this_uav_id", this_uav_id_int, 0x01);
        nh_.param<bool>("TX_debug", TX_debug, false);
        nh_.param<bool>("RX_debug", RX_debug, false);

        ground_station_addr_ = static_cast<uint32_t>(ground_addr_int);
        this_uav_id_ = static_cast<uint8_t>(this_uav_id_int);
        
        std::vector<int> other_uav_addrs_int;
        nh_.param<std::vector<int>>("other_uav_addrs", other_uav_addrs_int, {0x000217, 0x000317});
        for (int addr : other_uav_addrs_int) {
            other_uav_addrs_.push_back(static_cast<uint32_t>(addr));
        }
        
        nh_.param<std::string>("mavros_pose_topic", mavros_pose_topic_, "/mavros/local_position/pose");
        nh_.param<std::string>("detect_pose_topic", detect_pose_topic_, "/detect/local_position/pose");
        nh_.param<std::string>("battery_topic", battery_topic_, "/mavros/battery");
        
        nh_.param<std::vector<std::string>>("other_uav_mavros_topics", other_uav_mavros_topics_, 
                                            {"/uav2/mavros/local_position/pose", "/uav3/mavros/local_position/pose"});
        nh_.param<std::vector<std::string>>("other_uav_detect_topics", other_uav_detect_topics_, 
                                            {"/uav2/detect/local_position/pose", "/uav3/detect/local_position/pose"});
        
        nh_.param<double>("ground_station_freq", ground_station_freq_, 5.0);
        nh_.param<double>("other_uav_freq", other_uav_freq_, 10.0);
        nh_.param<double>("receive_freq", receive_freq_, 1.0);
        
        if (other_uav_addrs_.size() != other_uav_mavros_topics_.size() || 
            other_uav_addrs_.size() != other_uav_detect_topics_.size()) {
            ROS_ERROR("Inconsistent parameter sizes for other UAVs configuration");
            throw std::runtime_error("Parameter configuration error");
        }
        
        ROS_INFO("Ground Station Address: 0x%06X", ground_station_addr_);
        ROS_INFO("This UAV ID: 0x%02X", this_uav_id_);
        ROS_INFO("Battery Topic: %s", battery_topic_.c_str());
        ROS_INFO("Packet Size: %zu bytes (with battery and status)", PACKET_SIZE_SEND);
        
        for (size_t i = 0; i < other_uav_addrs_.size(); ++i) {
            ROS_INFO("Other UAV %zu - Address: 0x%06X", i, other_uav_addrs_[i]);
        }
    }

    void initializeSerial() {
        nh_.param<std::string>("serial_port", serial_port_name_, "/dev/ttyUSB0");
        nh_.param<int>("baudrate", serial_baudrate_, 115200);
        
        try {
            serial_port_.setPort(serial_port_name_);
            serial_port_.setBaudrate(serial_baudrate_);
            serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
            serial_port_.setTimeout(timeout);
            serial_port_.setBytesize(serial::eightbits);
            serial_port_.setParity(serial::parity_none);
            serial_port_.setStopbits(serial::stopbits_one);
            serial_port_.open();
            
            if (serial_port_.isOpen()) {
                ROS_INFO("Serial port %s opened successfully", serial_port_name_.c_str());
            } else {
                ROS_ERROR("Failed to open serial port %s", serial_port_name_.c_str());
            }
        } catch (serial::IOException& e) {
            ROS_ERROR("Serial port initialization failed: %s", e.what());
        }
    }
    
    // 重新打开串口函数
    bool reconnectSerial() {
        ROS_WARN("Attempting to reconnect to serial port...");
        try {
            if (serial_port_.isOpen()) {
                serial_port_.close();
            }
            serial_port_.setPort(serial_port_name_);
            serial_port_.setBaudrate(serial_baudrate_);
            serial_port_.open();
            
            if (serial_port_.isOpen()) {
                ROS_INFO("Serial port reconnected successfully");
                return true;
            } else {
                ROS_ERROR("Failed to reconnect to serial port");
                return false;
            }
        } catch (const std::exception& e) {
            ROS_ERROR("Serial reconnection failed: %s", e.what());
            return false;
        }
    }
    
    void initializeROS() {
        mavros_pose_sub_ = nh_.subscribe(mavros_pose_topic_, 10, 
            &UAVSwarmCommunication::mavrosPoseCallback, this);
        detect_pose_sub_ = nh_.subscribe(detect_pose_topic_, 10, 
            &UAVSwarmCommunication::detectPoseCallback, this);
        battery_sub_ = nh_.subscribe(battery_topic_, 10,
            &UAVSwarmCommunication::batteryCallback, this);
        
        for (size_t i = 0; i < other_uav_addrs_.size(); ++i) {
            ros::Publisher mavros_pub = nh_.advertise<geometry_msgs::PoseStamped>(other_uav_mavros_topics_[i], 10);
            ros::Publisher detect_pub = nh_.advertise<geometry_msgs::PoseStamped>(other_uav_detect_topics_[i], 10);
            
            other_uav_mavros_pubs_.push_back(mavros_pub);
            other_uav_detect_pubs_.push_back(detect_pub);
        }
        
        // 为ID为04的控制数据创建发布者
        control_velocity_pub_ = nh_.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);
        control_mode_pub_ = nh_.advertise<std_msgs::Int32>("/control_mode", 10);
        
        ROS_INFO("Initialized %zu publishers for other UAVs", other_uav_addrs_.size());
        ROS_INFO("Initialized control publishers for ID 04 data");
    }
    
    void mavrosPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(pose_mutex_);
        current_mavros_pose_ = *msg;
        mavros_pose_received_ = true;
    }
    
    void detectPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(pose_mutex_);
        current_detect_pose_ = *msg;
        detect_pose_received_ = true;
    }
    
    void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(battery_mutex_);
        
        // 更新电池百分比 (0-100)
        battery_percentage_ = static_cast<uint8_t>(msg->percentage * 100);
        
        // 设置状态标志位
        status_flags_ = 0;
        
        // 位0: 电池是否正常 (1=正常, 0=警告)
        if (msg->percentage > 0.2) {
            status_flags_ |= 0x01;
        }
        
        // 位1: 是否正在充电
        if (msg->power_supply_status == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_CHARGING) {
            status_flags_ |= 0x02;
        }
        
        // 位2: 是否连接外部电源
        if (msg->power_supply_status == sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING) {
            status_flags_ |= 0x04;
        }
        
        // 保留字节设为0
        reserved1_ = 0;
        reserved2_ = 0;
        
        if (TX_debug) {
            ROS_INFO("Battery updated: %d%%, Status: 0x%02X", battery_percentage_, status_flags_);
        }
    }
    
    void sendThread() {
        ros::Rate loop_rate(50);
        
        int ground_interval_ms = static_cast<int>(1000.0 / ground_station_freq_);
        int uav_interval_ms = static_cast<int>(1000.0 / other_uav_freq_);
        
        while (running_ && ros::ok()) {
            auto now = std::chrono::steady_clock::now();
            
            auto ground_duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_ground_send_);
            if (ground_duration.count() >= ground_interval_ms) {
                sendToTarget(ground_station_addr_);
                last_ground_send_ = now;
            }
            
            auto uav_duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_uav_send_);
            if (uav_duration.count() >= uav_interval_ms) {
                for (uint32_t addr : other_uav_addrs_) {
                    sendToTarget(addr);
                    std::this_thread::sleep_for(std::chrono::milliseconds(10));
                }
                last_uav_send_ = now;
            }
            
            loop_rate.sleep();
        }
    }
    
    void sendToTarget(uint32_t target_addr) {
        UAVPositionData data;
        {
            std::lock_guard<std::mutex> lock(pose_mutex_);
            if (TX_debug) std::cout << "mavros_pose_received_ = " << mavros_pose_received_ << std::endl;
            if (!mavros_pose_received_) return;
            
            data.target_address = target_addr;
            data.uav_id = this_uav_id_;
            data.mavros_pose = current_mavros_pose_;
            
            if (!detect_pose_received_) {
                data.detect_pose.pose.position.x = 0.0;
                data.detect_pose.pose.position.y = 0.0;
                data.detect_pose.pose.position.z = 0.0;
                data.detect_pose.pose.orientation.x = 0.0;
                data.detect_pose.pose.orientation.y = 0.0;
                data.detect_pose.pose.orientation.z = 0.0;
                data.detect_pose.pose.orientation.w = 1.0;
            } else {
                data.detect_pose = current_detect_pose_;
            }
        }
        
        // 添加电池和状态数据
        {
            std::lock_guard<std::mutex> lock(battery_mutex_);
            data.battery_percentage = battery_percentage_;
            data.status_flags = status_flags_;
            data.reserved1 = reserved1_;
            data.reserved2 = reserved2_;
        }

        std::vector<uint8_t> packet = serializeData(data);
        std::cout << "\033[32mSend : " << packet.size() << " bytes\033[0m" << std::endl;
        if (TX_debug) {
            
            std::stringstream ss;
            ss << "Packet content: ";
            for (uint8_t byte : packet) {
                ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
            }
            ROS_INFO("%s", ss.str().c_str());

            // 输出数据内容
            ROS_INFO("[sendToTarget] Target Addr: 0x%06X, UAV ID: %u", data.target_address, data.uav_id);
            ROS_INFO("[sendToTarget] MAVROS Pose: pos(%.3f, %.3f, %.3f), orientation(x, y, z, w)=%.3f, %.3f, %.3f, %.3f",
                    data.mavros_pose.pose.position.x,
                    data.mavros_pose.pose.position.y,
                    data.mavros_pose.pose.position.z,
                    data.mavros_pose.pose.orientation.x,
                    data.mavros_pose.pose.orientation.y,
                    data.mavros_pose.pose.orientation.z,
                    data.mavros_pose.pose.orientation.w);
            ROS_INFO("[sendToTarget] Detect Pose: pos(%.3f, %.3f, %.3f), orientation(x, y, z, w)=%.3f, %.3f, %.3f, %.3f",
                    data.detect_pose.pose.position.x,
                    data.detect_pose.pose.position.y,
                    data.detect_pose.pose.position.z,
                    data.detect_pose.pose.orientation.x,
                    data.detect_pose.pose.orientation.y,
                    data.detect_pose.pose.orientation.z,
                    data.detect_pose.pose.orientation.w);
            ROS_INFO("[sendToTarget] Battery: %d%%, Status: 0x%02X", 
                     data.battery_percentage, data.status_flags);
        }
        
        {
            std::lock_guard<std::mutex> lock(serial_mutex_);
            try {
                // 检查串口是否打开，如果没打开则尝试重连
                if (!serial_port_.isOpen()) {
                    ROS_WARN("Serial port not open, attempting to reconnect...");
                    if (!reconnectSerial()) {
                        ROS_ERROR("Reconnect failed, skipping this send");
                        return;
                    }
                }
                
                size_t bytes_written = serial_port_.write(packet);
                if (bytes_written != packet.size()) {
                    ROS_WARN("Incomplete data sent. Expected: %zu, Actual: %zu", packet.size(), bytes_written);
                    
                    // 写入失败后尝试重连
                    ROS_WARN("Attempting to reconnect after incomplete write...");
                    if (reconnectSerial()) {
                        // 重连后尝试重新发送
                        bytes_written = serial_port_.write(packet);
                        if (bytes_written != packet.size()) {
                            ROS_ERROR("Still incomplete write after reconnect: %zu/%zu", bytes_written, packet.size());
                        }
                    }
                }
            } catch (serial::IOException& e) {
                ROS_ERROR("Serial write error: %s", e.what());
                
                // 写入异常后尝试重连
                ROS_WARN("Attempting to reconnect after write exception...");
                reconnectSerial();
            } catch (serial::SerialException& e) {
                ROS_ERROR("Serial exception during write: %s", e.what());
                
                // 写入异常后尝试重连
                ROS_WARN("Attempting to reconnect after serial exception...");
                reconnectSerial();
            } catch (const std::exception& e) {
                ROS_ERROR("General exception during write: %s", e.what());
            }
        }
    }

    void receiveThread() {
        ros::Rate loop_rate(1.5);
        std::vector<uint8_t> buffer;
        
        while (running_ && ros::ok()) {
            {
                std::lock_guard<std::mutex> lock(serial_mutex_);
                // 检查串口是否打开，如果没打开则尝试重连
                if (!serial_port_.isOpen()) {
                    ROS_WARN_THROTTLE(1, "Serial port not open, attempting to reconnect...");
                    if (reconnectSerial()) {
                        ROS_INFO("Serial port reconnected in receive thread");
                    } else {
                        ROS_WARN_THROTTLE(1, "Reconnect failed in receive thread");
                    }
                }
            }
            
            try {
                std::lock_guard<std::mutex> lock(serial_mutex_);
                if (!serial_port_.isOpen()) {
                    loop_rate.sleep();
                    continue;
                }
                
                size_t available = serial_port_.available();
                if (available > 0) {
                    std::vector<uint8_t> temp_buffer(available);
                    size_t bytes_read = serial_port_.read(temp_buffer.data(), available);
                    std::cout << "\033[33mReceived " << bytes_read << " bytes! \033[0m" << std::endl;
                    
                    if (RX_debug && bytes_read > 0) {
                        std::stringstream ss;
                        for (size_t i = 0; i < bytes_read; ++i) {
                            ss << "0x" << std::hex << std::setw(2) << std::setfill('0') 
                               << static_cast<int>(temp_buffer[i]) << " ";
                        }
                        ROS_INFO("Raw data: %s", ss.str().c_str());
                    }

                    // 将新数据追加到缓冲区
                    buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_read);
                    
                    // 处理缓冲区中的所有完整数据包
                    processAllPacketsInBuffer(buffer);
                }
            } catch (serial::IOException& e) {
                ROS_ERROR("Serial read error: %s", e.what());
                
                // 读取异常后尝试重连
                ROS_WARN("Attempting to reconnect after read exception...");
                reconnectSerial();
            } catch (serial::SerialException& e) {
                ROS_ERROR("Serial exception during read: %s", e.what());
                
                // 读取异常后尝试重连
                ROS_WARN("Attempting to reconnect after serial exception...");
                reconnectSerial();
            } catch (const std::exception& e) {
                ROS_ERROR("General exception during read: %s", e.what());
            }
            loop_rate.sleep();
        }
    }
    
    void processAllPacketsInBuffer(std::vector<uint8_t>& buffer) {
        size_t processed_packets = 0;
        
        while (buffer.size() >= 5) { // 至少需要帧头(2) + ID(1) + 校验(1) + 帧尾(2) = 6字节，这里保守估计5字节
            bool found_packet = false;
            size_t packet_start = 0;
            
            // 查找帧头 0xAA 0x55
            for (size_t i = 0; i <= buffer.size() - 3; ++i) {
                if (buffer[i] == 0xAA && buffer[i + 1] == 0x55) {
                    packet_start = i;
                    
                    // 根据ID确定包大小和处理方式
                    uint8_t packet_id = buffer[i + 2];
                    size_t expected_packet_size;
                    
                    if (packet_id == 0x04) {
                        expected_packet_size = PACKET_SIZE_CONTROL;
                    } else if (packet_id >= 0x01 && packet_id <= 0x03) {
                        expected_packet_size = PACKET_SIZE_NORMAL;
                    } else {
                        // 未知ID，跳过这个帧头
                        buffer.erase(buffer.begin() + i, buffer.begin() + i + 1);
                        continue;
                    }
                    
                    // 检查是否有足够的数据来构成完整包
                    if (packet_start + expected_packet_size > buffer.size()) {
                        // 数据不够，等待更多数据
                        break;
                    }

                    // 检查包尾
                    if (buffer[packet_start + expected_packet_size - 2] != 0x55 ||
                        buffer[packet_start + expected_packet_size - 1] != 0xAA) {
                        if (RX_debug) {
                            ROS_WARN("Invalid packet tail for ID 0x%02X at position %zu", packet_id, packet_start);
                            ROS_WARN("tail1= 0x%02X tail2= 0x%02X", buffer[packet_start + expected_packet_size - 2], buffer[packet_start + expected_packet_size - 1]);
                        }
                        buffer.erase(buffer.begin() + i, buffer.begin() + i + 1);
                        continue;
                    }
                    
                    // 计算校验和
                    uint8_t checksum = 0;
                    for (size_t j = packet_start; j < packet_start + expected_packet_size - 3; ++j) {
                        checksum ^= buffer[j];
                    }
                    
                    uint8_t expected_checksum = buffer[packet_start + expected_packet_size - 3];
                    if (checksum != expected_checksum) {
                        if (RX_debug) {
                            ROS_ERROR("Checksum error for ID 0x%02X. Expected: 0x%02X, Got: 0x%02X",
                                    packet_id, checksum, expected_checksum);
                        }
                        buffer.erase(buffer.begin() + i, buffer.begin() + i + 1);
                        continue;
                    }
                    
                    // 校验通过，处理数据包
                    if (RX_debug) {
                        ROS_INFO("\033[32mValid packet (ID: 0x%02X) received at position %zu:\033[0m", packet_id, packet_start);
                        std::ostringstream oss;
                        for (size_t j = packet_start; j < packet_start + expected_packet_size; ++j) {
                            oss << std::hex << std::uppercase
                                << std::setw(2) << std::setfill('0')
                                << static_cast<int>(buffer[j]) << " ";
                        }
                        ROS_INFO("%s", oss.str().c_str());
                    }
                    
                    // 根据ID类型解析数据
                    bool parse_success = false;
                    if (packet_id == 0x04) {
                        parse_success = parseControlPacket(buffer, packet_start);
                    } else {
                        parse_success = parseNormalDataPacket(buffer, packet_start);
                    }
                    
                    if (parse_success) {
                        processed_packets++;
                        if (RX_debug) ROS_INFO("Successfully parsed and published data for ID 0x%02X", packet_id);
                    }
                    
                    // 移除已处理的数据包
                    buffer.erase(buffer.begin() + packet_start, buffer.begin() + packet_start + expected_packet_size);
                    found_packet = true;
                    break;
                }
            }
            
            if (!found_packet) {
                // 没有找到有效的帧头，保留最后几个字节以防数据跨缓冲区
                if (buffer.size() > 2) {
                    buffer.erase(buffer.begin(), buffer.end() - 2);
                }
                break;
            }
        }
        
        if (RX_debug && processed_packets > 0) {
            ROS_INFO("Processed %zu packets in this cycle", processed_packets);
        }
    }
    
    bool parseControlPacket(const std::vector<uint8_t>& buffer, size_t start_pos) {
        // 控制包格式: 2帧头 + 1ID + 13数据 + 2待定 + 1校验 + 2帧尾 = 19字节
        if (start_pos + PACKET_SIZE_CONTROL > buffer.size()) return false;
        
        ReceivedControlData control_data;
        control_data.uav_id = buffer[start_pos + 2];
        
        // 解析半精度浮点数据: v_x, v_y, v_z, yaw, reserved1, reserved2 (共12字节)
        control_data.v_x = parseHalf(buffer, start_pos + 3);
        control_data.v_y = parseHalf(buffer, start_pos + 5);
        control_data.v_z = parseHalf(buffer, start_pos + 7);
        control_data.yaw = parseHalf(buffer, start_pos + 9);
        control_data.reserved1 = parseHalf(buffer, start_pos + 11);
        control_data.reserved2 = parseHalf(buffer, start_pos + 13);
        
        // MODE 是整数 (1字节)
        control_data.mode = static_cast<int32_t>(buffer[start_pos + 15]);
        
        if (RX_debug) {
            ROS_INFO("[parseControlPacket] Control Data from ID: 0x%02X", control_data.uav_id);
            ROS_INFO("[parseControlPacket] Velocity: vx=%.3f, vy=%.3f, vz=%.3f, yaw=%.3f",
                    control_data.v_x, control_data.v_y, control_data.v_z, control_data.yaw);
            ROS_INFO("[parseControlPacket] Mode: %d", control_data.mode);
        }
        
        // 发布控制数据
        // publishControlData(control_data);
        return true;
    }
    
    bool parseNormalDataPacket(const std::vector<uint8_t>& buffer, size_t start_pos) {
        // 普通数据包格式: 2帧头 + 1ID + 14uav位姿数据 + 14detect位姿数据 + 2电池/状态 + 2待定 + 1校验 + 2帧尾 = 38字节
        if (start_pos + PACKET_SIZE_NORMAL > buffer.size()) return false;
        
        ReceivedData data;
        
        // 解析源机地址 (根据ID推断地址)
        data.source_address = (static_cast<uint32_t>(0x00) << 16) |
                            (static_cast<uint32_t>(buffer[start_pos + 2]) << 8) |
                            static_cast<uint32_t>(0x17);
        
        if (RX_debug) {
            ROS_INFO("[parseNormalDataPacket] Received packet from source_address: 0x%06X", data.source_address);
        }

        // 检查是否来自其他无人机
        auto it = std::find(other_uav_addrs_.begin(), other_uav_addrs_.end(), data.source_address);
        if (it == other_uav_addrs_.end()) {
            if (RX_debug) {
                ROS_INFO("Received data from unknown source: 0x%06X", data.source_address);
            }
            return true;  // 包格式正确但不是目标数据
        }

        // UAV ID
        data.uav_id = buffer[start_pos + 2];

        // 解析 MAVROS 位置
        data.mavros_pose = parsePoseHalf(buffer, start_pos + 3);

        // 解析检测位置
        data.detect_pose = parsePoseHalf(buffer, start_pos + 17);

        // 解析电池和状态数据 (4字节)
        data.battery_percentage = buffer[start_pos + 31];
        data.status_flags = buffer[start_pos + 32];
        data.reserved1 = buffer[start_pos + 33];
        data.reserved2 = buffer[start_pos + 34];

        // 打印详细信息
        if (RX_debug) {
            ROS_INFO("[parseNormalDataPacket] Source UAV Addr: 0x%06X, UAV ID: %u",
                    data.source_address, data.uav_id);
                    
            ROS_INFO("[parseNormalDataPacket] MAVROS Pose: pos(%.3f, %.3f, %.3f), orientation(x, y, z, w)=%.3f, %.3f, %.3f, %.3f",
                    data.mavros_pose.pose.position.x,
                    data.mavros_pose.pose.position.y,
                    data.mavros_pose.pose.position.z,
                    data.mavros_pose.pose.orientation.x,
                    data.mavros_pose.pose.orientation.y,
                    data.mavros_pose.pose.orientation.z,
                    data.mavros_pose.pose.orientation.w);
            
            ROS_INFO("[parseNormalDataPacket] Detect Pose: pos(%.3f, %.3f, %.3f), orientation(x, y, z, w)=%.3f, %.3f, %.3f, %.3f",
                    data.detect_pose.pose.position.x,
                    data.detect_pose.pose.position.y,
                    data.detect_pose.pose.position.z,
                    data.detect_pose.pose.orientation.x,
                    data.detect_pose.pose.orientation.y,
                    data.detect_pose.pose.orientation.z,
                    data.detect_pose.pose.orientation.w);
            
            ROS_INFO("[parseNormalDataPacket] Battery: %d%%, Status: 0x%02X", 
                     data.battery_percentage, data.status_flags);
        }

        // 发布数据
        publishReceivedData(data);
        return true;
    }

    std::vector<uint8_t> serializeData(const UAVPositionData& data) {
        std::vector<uint8_t> packet;
        
        // 目标地址 (3字节)
        packet.push_back((data.target_address >> 16) & 0xFF);
        packet.push_back((data.target_address >> 8) & 0xFF);
        packet.push_back(data.target_address & 0xFF);

        // 数据包头标识 （2字节）
        packet.push_back(0xAA);
        packet.push_back(0x55);
        
        // 本机ID（1字节）
        packet.push_back(data.uav_id);
        
        // MAVROS位置数据 (使用半精度，14字节)
        serializePoseHalf(packet, data.mavros_pose);
        
        // 检测位置数据 (使用半精度，14字节)
        serializePoseHalf(packet, data.detect_pose);
        
        // 添加电池和状态数据 (4字节)
        packet.push_back(data.battery_percentage);
        packet.push_back(data.status_flags);
        packet.push_back(data.reserved1);
        packet.push_back(data.reserved2);
        
        // 校验和 (从帧头开始计算)（1字节）
        uint8_t checksum = 0;
        for (size_t i = 3; i < packet.size(); ++i) {
            checksum ^= packet[i];
        }
        packet.push_back(checksum);
        
        // 数据包尾标识（2字节）
        packet.push_back(0x55);
        packet.push_back(0xAA);
        
        return packet;
    }

    void serializePoseHalf(std::vector<uint8_t>& packet, const geometry_msgs::PoseStamped& pose) {
        const auto& pos = pose.pose.position;
        const auto& orient = pose.pose.orientation;
        
        // 位置 (6字节，每个坐标2字节)
        appendHalf(packet, pos.x);
        appendHalf(packet, pos.y);
        appendHalf(packet, pos.z);
        
        // 四元数 (8字节，每个分量2字节)
        appendHalf(packet, orient.x);
        appendHalf(packet, orient.y);
        appendHalf(packet, orient.z);
        appendHalf(packet, orient.w);
    }
    
    void appendHalf(std::vector<uint8_t>& packet, float value) {
        uint16_t half_value = floatToHalf(value);
        packet.push_back(half_value & 0xFF);
        packet.push_back((half_value >> 8) & 0xFF);
    }
    
    geometry_msgs::PoseStamped parsePoseHalf(const std::vector<uint8_t>& buffer, size_t start_pos) {
        geometry_msgs::PoseStamped pose;
        pose.header.stamp = ros::Time::now();
        pose.header.frame_id = "map";
        
        // 解析位置 (半精度)
        pose.pose.position.x = parseHalf(buffer, start_pos);
        pose.pose.position.y = parseHalf(buffer, start_pos + 2);
        pose.pose.position.z = parseHalf(buffer, start_pos + 4);
        
        // 解析四元数 (半精度)
        pose.pose.orientation.x = parseHalf(buffer, start_pos + 6);
        pose.pose.orientation.y = parseHalf(buffer, start_pos + 8);
        pose.pose.orientation.z = parseHalf(buffer, start_pos + 10);
        pose.pose.orientation.w = parseHalf(buffer, start_pos + 12);
        
        return pose;
    }
    
    float parseHalf(const std::vector<uint8_t>& buffer, size_t start_pos) {
        uint16_t half_value = static_cast<uint16_t>(buffer[start_pos]) | 
                             (static_cast<uint16_t>(buffer[start_pos + 1]) << 8);
        return halfToFloat(half_value);
    }
    
    void publishReceivedData(const ReceivedData& data) {
        auto it = std::find(other_uav_addrs_.begin(), other_uav_addrs_.end(), data.source_address);
        if (it != other_uav_addrs_.end()) {
            size_t index = std::distance(other_uav_addrs_.begin(), it);
            
            other_uav_mavros_pubs_[index].publish(data.mavros_pose);
            other_uav_detect_pubs_[index].publish(data.detect_pose);
            
            if (RX_debug) {
                ROS_INFO("Published data for UAV 0x%06X: pos(%.2f,%.2f,%.2f) battery=%d%% status=0x%02X", 
                        data.source_address,
                        data.mavros_pose.pose.position.x,
                        data.mavros_pose.pose.position.y,
                        data.mavros_pose.pose.position.z,
                        data.battery_percentage,
                        data.status_flags);
            }
        }
    }

};

int main(int argc, char** argv) {
    ros::init(argc, argv, "uav_swarm_communication");
    
    try {
        UAVSwarmCommunication comm;
        ros::spin();
    } catch (const std::exception& e) {
        ROS_ERROR("Exception in main: %s", e.what());
        return 1;
    }
    
    return 0;
}